/******************************************************************************
* Copyright Authors DustinKsi@ Youibot 2019. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/

#pragma once

#include "gtest/gtest_prod.h"
#include "venus/mapping/interface/abstract_mapping.h"

#include <map>
#include <queue>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include <cmath>
#include <sstream>
#include <string>

#include <opencv2/opencv.hpp>

using std::pair;
using std::make_pair;
using std::vector;
using std::map;
using std::unordered_map;
using std::unordered_set;
using std::priority_queue;

using std::min;
using std::max;
using std::stringstream;

namespace VEnus {
namespace Mapping {
class FullGraphMapping : public AbstractMapping {
 private:
  int32_t _g_feature_id_cnt;

  struct EdgeWithLen {
    int32_t greator_id;
    int32_t less_id;
    double edge_length;

    EdgeWithLen(int32_t p_id, int32_t c_id, double len) {
      CHECK_NE(p_id, c_id) << "parent's id should not equal with child's id.";
      greator_id = max(p_id, c_id);
      less_id = min(p_id, c_id);
      edge_length = len;
    }

    bool operator<(const EdgeWithLen &fedge) const { return fedge.edge_length < edge_length; }
  };

  unordered_map<int, pair<double, double> > _g_fpts_list;

  map<pair<int, int>, double> _g_distance_dict;
  priority_queue<EdgeWithLen> _g_edges_heap;
  unordered_map<int, int> _g_graph;

 public:
  ~FullGraphMapping();

  bool Init();

  bool InsertFeatureList(VEnus::Sensor::Feature2DList &feature_list,
                         VEnus::Sensor::AdjacencyList &adjacency_list,
                         VEnus::Sensor::RobotPose &pose);

  bool GenerateListGraphAndHeap(unordered_map<int, pair<double, double> > &fpts_list,
                                map<pair<int, int>, double> &distance_dict,
                                priority_queue<EdgeWithLen> &edges_heap,
                                unordered_map<int, int> &graph);

  bool UpdateEdgeList(unordered_map<int, pair<double, double> > &fpts_list);

  bool ExtractKeyEdge(unordered_map<int, pair<double, double> > &fpts_list,
                      unordered_map<int, double> &score_dict);

  void GenerateScoreGraphImage(unordered_map<int, pair<double, double> > &fpts_list,
                               unordered_map<int, double> score_dict, cv::Mat &graph_image);

  void GenerateGraphImage(unordered_map<int, pair<double, double> > &fpts_list,
                          map<pair<int, int>, double> &distance_dict, cv::Mat &graph_image);
  
  bool SetInitialPose(VEnus::Sensor::RobotPose &pose);

 private:
  friend class MappingFactory;

  FRIEND_TEST(FullGraphMappingTest, GenerateGraphAndHeap);
  FRIEND_TEST(FullGraphMappingTest, GraphMatch);

  FullGraphMapping();
};

}  // namespace Mapping

}  // namespace VEnus
